import log.Log;

import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;

import net.sf.json.JSONObject;
import net.sf.json.JSONSerializer;


public class Robot {
	private static final double LOOK_AHEAD = 0.625; 
	private DifferentialDrive differentialDrive;
	private LaserScan sensors;
	private Pose pose;
	private Path pathToFollow;
	private double lookAheadDistance;
	
	public Robot(DifferentialDrive differentialDrive, LaserScan sensors,
			Pose pose, Path pathToFollow) {
		super();
		this.differentialDrive = differentialDrive;
		this.sensors = sensors;
		this.pose = pose;
		this.pathToFollow = pathToFollow;
		lookAheadDistance = LOOK_AHEAD;
	}
	
	public Robot(){
		super();
		pose = new Pose();
		DifferentialDriveProperties dp = NetworkCommunication.initDifferentialDrivePropertiesFromServer();
		differentialDrive = new DifferentialDrive(new DifferentialDriveCommand(0, 0), new DifferentialDriveFeedback(0, 0, true), dp, 0);
		differentialDrive.getCommand().setProperties(dp);
		lookAheadDistance = LOOK_AHEAD;
	}
	
	public DifferentialDrive getDifferentialDrive() {
		return differentialDrive;
	}
	public void setDifferentialDrive(DifferentialDrive differentialDrive) {
		this.differentialDrive = differentialDrive;
	}
	public LaserScan getSensors() {
		return sensors;
	}
	public void setSensors(LaserScan sensors) {
		this.sensors = sensors;
	}
	public Pose getPose() {
		return pose;
	}
	public void setPose(Pose pose) {
		this.pose = pose;
	}
	public Path getPathToFollow() {
		return pathToFollow;
	}
	public void setPathToFollow(Path pathToFollow) {
		this.pathToFollow = pathToFollow;
	}
	
	public double getLookAheadDistance() {
		return lookAheadDistance;
	}
	
	/**
	 * Retrieve robot pose from server
	 */
	public void getCurrentPose(){
		JSONObject obj = NetworkCommunication.getRobotPositionFromServer();
		pose.setPoseFromJSONObject(obj);
	}
	
	public void getDifferentialDriveFromNetwork(){
		JSONObject obj = NetworkCommunication.getRobotDifferentialDriveFromServer();
		differentialDrive.setDifferentialDriveFromJSONObject(obj); 
	}
	
	public long estimateTimeOfArrivalLinear(double distance){
		return (long) (1000 * (1 / (Math.abs(this.differentialDrive.getCommand().getTargetLinearSpeed() / distance))));
	}
	
	public long estimateTimeOfArrivalAngular(double distance){
		return (long) (1000 * (1 / (Math.abs(this.differentialDrive.getCommand().getTargetAngularSpeed() / distance))));
	}

	public void calulateLookAheadDistance(Path p){
		int nbPointBefore = 0;
		Vector3D toCompare;
		for(int i = 0; i < p.getPath().size(); i++){
			toCompare = p.getPath().get(i).convertPositionToRobotCoordinateSystem(this);
			if(toCompare.getNorm() < 1){
				nbPointBefore++;
			}
		}
		lookAheadDistance = (p.getPath().size() - nbPointBefore)/p.getPath().size();
	}
	
	public void followTheCarrot(double angle, Vector3D goalPoint){
		long eta;
		double distance = goalPoint.getNorm();
		this.getDifferentialDrive().getCommand().setTargetAngularSpeed(this.getDifferentialDrive().getProperties().getMaxAngularSpeed());//nearestPoint.getX()/nearestPoint.getY()));
		eta = this.estimateTimeOfArrivalAngular(angle);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
		try {
			Thread.sleep(eta);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		stop();
		this.getDifferentialDrive().getCommand().setTargetLinearSpeed(this.getDifferentialDrive().getProperties().getMaxLinearSpeed());
		eta = this.estimateTimeOfArrivalLinear(distance);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
		try {
			Thread.sleep(eta);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		stop();
	}
	
	public void purePursuit(double angle, Vector3D goalPoint){
		long eta;
		double radius = 1/ (2 * (goalPoint.getY()/Math.pow(goalPoint.getNorm(), 2)));
		double distance = 2*Math.PI*radius / ((2*Math.PI)/angle);
		this.getDifferentialDrive().getCommand().setTargetLinearSpeed(distance);
		eta = this.estimateTimeOfArrivalLinear(distance);
		double angularSpeed = 1000 * angle / eta;
		this.getDifferentialDrive().getCommand().setTargetAngularSpeed(angularSpeed);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
		try{
			Thread.sleep(eta);
		}catch(InterruptedException e){
			e.printStackTrace();
		}
		stop();
		
	}
	
	private void stop(){
		this.getDifferentialDrive().getCommand().setTargetAngularSpeed(0);
		this.getDifferentialDrive().getCommand().setTargetLinearSpeed(0);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
	}
}
